/**
* This file is part of Structure-SLAM.
* Copyright (C) 2020 Yanyan Li <yanyan.li at tum.de> (Technical University of Munich)
*
*/

#include "MapLine.h"

#include <mutex>
#include <map>

using namespace std;
using namespace cv;
using namespace cv::line_descriptor;
using namespace Eigen;

namespace ORB_SLAM2 {
    mutex MapLine::mGlobalMutex;
    long unsigned int MapLine::nNextId = 0;

    MapLine::MapLine(Vector6d &Pos, KeyFrame *pRefKF, Map *pMap) :
            mnFirstKFid(pRefKF->mnId), nObs(0), mnTrackReferenceForFrame(0),
            mnLastFrameSeen(0), mnFuseCandidateForKF(0), mpRefKF(pRefKF), mnVisible(1), mnFound(1), mbBad(false),
            mpReplaced(static_cast<MapLine *>(NULL)), mpMap(pMap) {
        mWorldPos = Pos;

        mNormalVector << 0, 0, 0;

        // MapLines can be created from Tracking and Local Mapping. This mutex avoid conflicts with id.
        unique_lock<mutex> lock(mpMap->mMutexPointCreation);
        mnId = nNextId++;
    }

    MapLine::MapLine(Vector6d &Pos, Map *pMap, Frame *pFrame, const int &idxF) :
            mnFirstKFid(-1), nObs(0), mnTrackReferenceForFrame(0), mnLastFrameSeen(0),
            mnFuseCandidateForKF(0), mpRefKF(static_cast<KeyFrame *>(NULL)), mnVisible(1),
            mnFound(1), mbBad(false), mpReplaced(NULL), mpMap(pMap) {
        mWorldPos = Pos;
        Mat Ow = pFrame->GetCameraCenter();
        Vector3d OW;
        OW << Ow.at<double>(0), Ow.at<double>(1), Ow.at<double>(2);
        mStart3D = Pos.head(3);
        mEnd3D = Pos.tail(3);
        Vector3d midPoint = 0.5 * (mStart3D + mEnd3D);
        mNormalVector = midPoint - OW;
        mNormalVector.normalize();

        Vector3d PC = midPoint - OW;
        const float dist = PC.norm();

        const int level = pFrame->mvKeylinesUn[idxF].octave;
        const float levelScaleFactor = pFrame->mvScaleFactors[level];
        const int nLevels = pFrame->mnScaleLevels;

        mfMaxDistance = dist * levelScaleFactor;
        mfMinDistance = mfMaxDistance / pFrame->mvScaleFactors[nLevels - 1];

        pFrame->mLdesc.row(idxF).copyTo(mLDescriptor);

        // MapLines can be created from Tracking and Local Mapping. This mutex avoid conflicts with id.
        unique_lock<mutex> lock(mpMap->mMutexLineCreation);
        mnId = nNextId++;
    }

    Vector6d MapLine::GetWorldPos() {
        unique_lock<mutex> lock(mMutexPos);
        return mWorldPos;
    }

    Vector3d MapLine::GetNormal() {
        unique_lock<mutex> lock(mMutexPos);
        return mNormalVector;
    }

    void MapLine::AddObservation(KeyFrame *pKF, size_t idx) {
        unique_lock<mutex> lock(mMutexFeatures);
        if (mObservations.count(pKF))
            return;

        mObservations[pKF] = idx;

        nObs++;
    }

    void MapLine::EraseObservation(KeyFrame *pKF) {
        bool bBad = false;
        {
            unique_lock<mutex> lock(mMutexFeatures);
            if (mObservations.count(pKF)) {
                mObservations.erase(pKF);
                nObs--;

                if (mpRefKF == pKF)
                    mpRefKF = mObservations.begin()->first;

                if (nObs <= 2)
                    bBad = true;
            }
        }

        if (bBad)
            SetBadFlag();
    }

    int MapLine::Observations() {
        unique_lock<mutex> lock(mMutexFeatures);
        return nObs;
    }

    bool MapLine::IsInKeyFrame(KeyFrame *pKF) {
        unique_lock<mutex> lock(mMutexFeatures);
        return (mObservations.count(pKF));
    }

    void MapLine::SetBadFlag() {
        map<KeyFrame *, size_t> obs;
        {
            unique_lock<mutex> lock1(mMutexFeatures);
            unique_lock<mutex> lock2(mMutexPos);
            mbBad = true;
            obs = mObservations;
            mObservations.clear();
        }

        for (auto &ob : obs) {
            KeyFrame *pKF = ob.first;
            pKF->EraseMapLineMatch(ob.second);
        }

        mpMap->EraseMapLine(this);
    }

    bool MapLine::isBad() {
        unique_lock<mutex> lock(mMutexFeatures);
        unique_lock<mutex> lock2(mMutexPos);
        return mbBad;
    }

    void MapLine::Replace(MapLine *pML) {
        if (pML->mnId == this->mnId)
            return;

        int nvisible, nfound;
        map<KeyFrame *, size_t> obs;
        {
            unique_lock<mutex> lock1(mMutexFeatures);
            unique_lock<mutex> lock2(mMutexPos);
            obs = mObservations;
            mObservations.clear();
            mbBad = true;
            nvisible = mnVisible;
            nfound = mnFound;
            mpReplaced = pML;
        }

        for (map<KeyFrame *, size_t>::iterator mit = obs.begin(), mend = obs.end(); mit != mend; mit++) {
            KeyFrame *pKF = mit->first;

            if (!pML->IsInKeyFrame(pKF)) {
                pKF->ReplaceMapLineMatch(mit->second, pML);
                pML->AddObservation(pKF, mit->second);
            } else {
                pKF->EraseMapLineMatch(mit->second);
            }
        }

        pML->IncreaseFound(nfound);
        pML->IncreaseVisible(nvisible);
        pML->ComputeDistinctiveDescriptors();

        mpMap->EraseMapLine(this);
    }

    MapLine *MapLine::GetReplaced() {
        unique_lock<mutex> lock1(mMutexFeatures);
        unique_lock<mutex> lock2(mMutexPos);
        return mpReplaced;
    }

    void MapLine::IncreaseVisible(int n) {
        unique_lock<mutex> lock(mMutexFeatures);
        mnVisible += n;
    }

    void MapLine::IncreaseFound(int n) {
        unique_lock<mutex> lock(mMutexFeatures);
        mnFound += n;
    }

    float MapLine::GetFoundRatio() {
        unique_lock<mutex> lock(mMutexFeatures);
        return static_cast<float>(mnFound / mnVisible);
    }

    void MapLine::ComputeDistinctiveDescriptors() {
        // Retrieve all observed descriptors
        vector<Mat> vDescriptors;

        map<KeyFrame *, size_t> observations;
        {
            unique_lock<mutex> lock1(mMutexFeatures);
            if (mbBad)
                return;
            observations = mObservations;
        }

        if (observations.empty())
            return;

        vDescriptors.reserve(observations.size());

        for (map<KeyFrame *, size_t>::iterator mit = observations.begin(), mend = observations.end();
             mit != mend; mit++) {
            KeyFrame *pKF = mit->first;

            if (!pKF->isBad())
                vDescriptors.push_back(pKF->mLineDescriptors.row(mit->second));
        }

        if (vDescriptors.empty())
            return;

        const size_t NL = vDescriptors.size();

        std::vector<std::vector<float>> Distances;
        Distances.resize(NL, vector<float>(NL, 0));
        for (size_t i = 0; i < NL; i++) {
            Distances[i][i] = 0;
            for (size_t j = 0; j < NL; j++) {
                int distij = norm(vDescriptors[i], vDescriptors[j], NORM_HAMMING);
                Distances[i][j] = distij;
                Distances[j][i] = distij;
            }
        }

        // Take the descriptor with least median distance to the rest
        int BestMedian = INT_MAX;
        int BestIdx = 0;
        for (size_t i = 0; i < NL; i++) {
            vector<int> vDists(Distances[i].begin(), Distances[i].end());
            sort(vDists.begin(), vDists.end());

            int median = vDists[0.5 * (NL - 1)];

            if (median < BestMedian) {
                BestMedian = median;
                BestIdx = i;
            }
        }
        {
            unique_lock<mutex> lock(mMutexFeatures);
            mLDescriptor = vDescriptors[BestIdx].clone();
        }

    }

    Mat MapLine::GetDescriptor() {
        unique_lock<mutex> lock(mMutexFeatures);
        return mLDescriptor.clone();
    }

    void MapLine::UpdateAverageDir() {
        map<KeyFrame *, size_t> observations;
        KeyFrame *pRefKF;
        Vector6d Pos;
        {
            unique_lock<mutex> lock1(mMutexFeatures);
            unique_lock<mutex> lock2(mMutexPos);
            if (mbBad)
                return;
            observations = mObservations;
            pRefKF = mpRefKF;
            Pos = mWorldPos;
        }

        if (observations.empty())
            return;

        Vector3d normal(0, 0, 0);
        int n = 0;
        for (map<KeyFrame *, size_t>::iterator mit = observations.begin(), mend = observations.end();
             mit != mend; mit++) {
            KeyFrame *pKF = mit->first;
            Mat Owi = pKF->GetCameraCenter();
            Vector3d OWi(Owi.at<float>(0), Owi.at<float>(1), Owi.at<float>(2));
            Vector3d middlePos = 0.5 * (mWorldPos.head(3) + mWorldPos.tail(3));
            Vector3d normali = middlePos - OWi;
            normal = normal + normali / normali.norm();
            n++;
        }

        cv::Mat SP = (Mat_<float>(3, 1) << Pos(0), Pos(1), Pos(2));
        cv::Mat EP = (Mat_<float>(3, 1) << Pos(3), Pos(4), Pos(5));
        cv::Mat MP = 0.5 * (SP + EP);

        cv::Mat CM = MP - pRefKF->GetCameraCenter();
        const float dist = cv::norm(CM);
        const int level = pRefKF->mvKeyLines[observations[pRefKF]].octave;
        const float levelScaleFactor = pRefKF->mvScaleFactors[level];
        const int nLevels = pRefKF->mnScaleLevels;

        {
            unique_lock<mutex> lock3(mMutexPos);
            mfMaxDistance = dist * levelScaleFactor;
            mfMinDistance = mfMaxDistance / pRefKF->mvScaleFactors[nLevels - 1];
            mNormalVector = normal / n;
        }
    }

    float MapLine::GetMinDistanceInvariance() {
        unique_lock<mutex> lock(mMutexPos);
        return 0.8f * mfMinDistance;
    }

    float MapLine::GetMaxDistanceInvariance() {
        unique_lock<mutex> lock(mMutexPos);
        return 1.2f * mfMaxDistance;
    }

    int MapLine::PredictScale(const float &currentDist, const float &logScaleFactor) {
        float ratio;
        {
            unique_lock<mutex> lock3(mMutexPos);
            ratio = mfMaxDistance / currentDist;
        }

        return ceil(log(ratio) / logScaleFactor);
    }
}